#include "StdAfx.h"
#include "CoordSys.h"
#include "boost\foreach.hpp"

CCoordSys::CCoordSys(void)
  : ox_(0)
  , oy_(0)
  , os_(0)
  , oc_(1)
  , oa_(0)
{
}

CCoordSys::CCoordSys(double ox, double oy, double oa)
  : ox_(ox)
  , oy_(oy)
  , oa_(oa)
{
  os_ = sin(angle_math::radian(oa_));
  oc_ = cos(angle_math::radian(oa_));
}

CCoordSys::CCoordSys(double ox, double oy, double os, double oc, double oa)
  : ox_(ox)
  , oy_(oy)
  , os_(os)
  , oc_(oc)
  , oa_(oa)
{
}


CCoordSys::~CCoordSys(void)
{
}

void CCoordSys::Transform(double &x, double &y) const
{
  double bx = x;
  double by = y;

  x = bx*oc_ - by*os_ + oy_*os_ - ox_*oc_;
  y = bx*os_ + by*oc_ - oy_*oc_ - ox_*os_;
}

void CCoordSys::Transform(point_2& pt) const
{
  Transform(pt.x, pt.y);
}

void CCoordSys::Transform(segment_2 &s) const
{
  Transform(s.start);
  Transform(s.end);
}

void CCoordSys::Transform(angle_box_2 &ab) const
{
  int a = (int)(angle_math::normalize(ab.angle-oa_)+0.5);
  point_2 cp(ab.center_x, ab.center_y);
  Transform(cp);
  ab = angle_box_2(cp.x, cp.y, ab.half_width, ab.half_height, a);
}

void CCoordSys::Transform(ring_2 &r) const
{
  point_2 cp(r.center_x, r.center_y);
  Transform(cp);
  r = ring_2(cp.x, cp.y, r.radius);
}

void CCoordSys::Transform(arc_2 &a) const
{
  int sa = (int)(angle_math::normalize(a.start_angle-oa_)+0.5);
  int ea = (int)(angle_math::normalize(a.end_angle-oa_)+0.5);
  point_2 cp(a.center_x, a.center_y);
  Transform(cp);
  a = arc_2(cp.x, cp.y, a.radius, sa, ea);
}

void CCoordSys::Transform(annulus_2 &a) const
{
  point_2 cp(a.center_x, a.center_y);
  Transform(cp);
  a = annulus_2(cp.x, cp.y, a.start_radius, a.end_radius);
}

void CCoordSys::Transform(annulus_arc_2 &a) const
{
  int sa = (int)(angle_math::normalize(a.start_angle-oa_)+0.5);
  int ea = (int)(angle_math::normalize(a.end_angle-oa_)+0.5);
  point_2 cp(a.center_x, a.center_y);
  Transform(cp);
  a = annulus_arc_2(cp.x, cp.y, a.start_radius, a.end_radius, sa, ea);
}

void CCoordSys::Transform(polygon_2 &pg) const
{
  BOOST_FOREACH(point_2& v, pg)
  {
    Transform(v);
  }
}

void CCoordSys::Inverse(double &x, double &y) const
{
  double bx = x;
  double by = y;

  x = by*os_+bx*oc_+ox_;
  y = by*oc_-bx*os_+oy_; 
}

void CCoordSys::Inverse(segment_2 &s) const
{
  Inverse(s.start);
  Inverse(s.end);
}

void CCoordSys::Inverse(angle_box_2 &ab) const
{
  int a = (int)(angle_math::normalize(ab.angle+oa_)+0.5);
  point_2 cp(ab.center_x, ab.center_y);
  Inverse(cp);
  ab = angle_box_2(cp.x, cp.y, ab.half_width, ab.half_height, a);
}

void CCoordSys::Inverse(ring_2 &r) const
{
  point_2 cp(r.center_x, r.center_y);
  Inverse(cp);
  r = ring_2(cp.x, cp.y, r.radius);
}

void CCoordSys::Inverse(arc_2 &a) const
{
  int sa = (int)(angle_math::normalize(a.start_angle+oa_)+0.5);
  int ea = (int)(angle_math::normalize(a.end_angle+oa_)+0.5);
  point_2 cp(a.center_x, a.center_y);
  Inverse(cp);
  a = arc_2(cp.x, cp.y, a.radius, sa, ea);
}

void CCoordSys::Inverse(annulus_2 &a) const
{
  point_2 cp(a.center_x, a.center_y);
  Inverse(cp);
  a = annulus_2(cp.x, cp.y, a.start_radius, a.end_radius);
}

void CCoordSys::Inverse(annulus_arc_2 &a) const
{
  int sa = (int)(angle_math::normalize(a.start_angle+oa_)+0.5);
  int ea = (int)(angle_math::normalize(a.end_angle+oa_)+0.5);
  point_2 cp(a.center_x, a.center_y);
  Inverse(cp);
  a = annulus_arc_2(cp.x, cp.y, a.start_radius, a.end_radius, sa, ea);
}

void CCoordSys::Inverse(polygon_2 &pg) const
{
  BOOST_FOREACH(point_2& v, pg)
  {
    Inverse(v);
  }
}

void CCoordSys::Reset(void)
{
  ox_ = 0;
  oy_ = 0;
  os_ = 0;
  oc_ = 1;
  oa_ = 0;
}

void CCoordSys::SetCoordSys(double ox, double oy, double oa)
{
  ox_ = ox;
  oy_ = oy;
  oa_ = oa;
  os_ = sin(angle_math::radian(oa_));
  oc_ = cos(angle_math::radian(oa_));
}

void CCoordSys::SetCoordSys(double ox, double oy, double os, double oc, double oa)
{
  ox_ = ox;
  oy_ = oy;
  os_ = os;
  oc_ = oc;
  oa_ = oa;
}

void CCoordSys::SetOA(double val)
{
  oa_ = val;
  os_ = sin(angle_math::radian(oa_));
  oc_ = cos(angle_math::radian(oa_));
}
